#include "pll.h"
#include "../MATH/my_math.h"
#include "../PRINT/my_printf.h"

void chg_pll(){
    // CODA960CLKENB |= (1 << 3);  // PCLK operating mode
    
    PLLSETREG0 &= ~((0x3F << 18) | (0x3FF << 8) | (0xFF << 0));
    PLLSETREG0 |= (6 << 18) | (550 << 8) | (2 << 0);    // 修改PMS

    // CLKDIVREG0 &= ~((0x7 << 0) | (0x3F << 3)  | (0x3F << 9));
    // CLKDIVREG0 |= ((0 << 0) | ((2 - 1) << 3) | ((8 - 1) << 9));

    PWRMODE |= (1 << 15);
    CLKMODEREG0 |= (1 << 0);    // Update P,M,S values for PLL[0]
}

void print_pllsetreg(PLLSETREG *p){
    printf("SSCG_EN = %d\r\n", p->SSCG_EN);
    printf("PLLSETREGx.PD = %d\r\n", p->PD);
    printf("PLLSETREGx.PLLOUTDIV = %d\r\n", p->PLLOUTDIV);
    printf("PLLSETREGx.PDIV = %d\r\n", p->PDIV);
    printf("PLLSETREGx.MDIV = %d\r\n", p->MDIV);
    printf("PLLSETREGx.SDIV = %d\r\n", p->SDIV);
    printf("F: %d MHz\r\n", p->MDIV * 24 / (p->PDIV * my_pow(2, p->SDIV)));
}

void print_clkdivreg0(CLKDIVREG0_STRUCT *p){
    printf("CLKDIVREG0: 0x%x\r\n", CLKDIVREG0);
    printf("SEL = %d\r\n", p->CLKSEL_FCLKCPU0);
    printf("CLKDIV_FCLK = %d\r\n", (CLKDIVREG0 >> 3) & 0x3F);
    printf("CLKDIV_HCLK = %d\r\n", (CLKDIVREG0 >> 9) & 0x3F);
}

void print_clkdivreg1(CLKDIVREG1_STRUCT *p){
    printf("CLKDIVREG1: 0x%x\r\n", CLKDIVREG1);
    printf("SEL = %d\r\n", p->CLKSEL_BCLK);
    printf("CLKDIV_BCLK = %d\r\n", (CLKDIVREG1 >> 3) & 0x3F);
    printf("CLKDIV_PCLK = %d\r\n", (CLKDIVREG1 >> 9) & 0x3F);
}

void print_PLL(){
    printf("--------------------------%c\r\n", '-');
    printf("PLL0: 0x%x\r\n", PLLSETREG0);
    print_pllsetreg((PLLSETREG *)(&PLLSETREG0));
    printf("--------------------------%c\r\n", '-');
    printf("PLL1: 0x%x\r\n", PLLSETREG1);
    print_pllsetreg((PLLSETREG *)(&PLLSETREG1));
    printf("--------------------------%c\r\n", '-');
    printf("PLL2: 0x%x\r\n", PLLSETREG2);
    print_pllsetreg((PLLSETREG *)(&PLLSETREG2));
    printf("--------------------------%c\r\n", '-');
    printf("PLL3: 0x%x\r\n", PLLSETREG3);
    print_pllsetreg((PLLSETREG *)(&PLLSETREG3));

    printf("--------------------------%c\r\n", '-');
    print_clkdivreg0((CLKDIVREG0_STRUCT *)(&CLKDIVREG0));
    printf("--------------------------%c\r\n", '-');
    print_clkdivreg1((CLKDIVREG1_STRUCT *)(&CLKDIVREG1));
}